// Copyright (C) 2023 Thies Lennart Alff
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 2
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301,
// USA

#include "hippo_control/trajectory_tracking_bluerov/tracking_controller_bluerov.hpp"

namespace hippo_control {
namespace trajectory_tracking_bluerov {

TrackingControllerBlueROVNode::TrackingControllerBlueROVNode(
    rclcpp::NodeOptions const &_options)
    : Node("tracking_controller_bluerov", _options) { //基础节点名称（会被launch文件覆盖）
  DeclareParams();
  InitPublishers();
  InitSubscriptions();
}

void TrackingControllerBlueROVNode::InitPublishers() {
  std::string topic;

  topic = "thrust_attitude_target";  // 修改：使用新的topic名称
  attitude_target_pub_ =
      create_publisher<hippo_control_msgs::msg::ThrustAttitudeTarget>(topic, 10);  // 修改：使用新的消息类型
}


void TrackingControllerBlueROVNode::InitSubscriptions() {
  std::string topic;

  topic = "attitude_target_desired";
  attitude_target_desired_sub_ = create_subscription<geometry_msgs::msg::QuaternionStamped>(
      topic, 10, [this](const geometry_msgs::msg::QuaternionStamped::SharedPtr _msg) {
        OnAttitudeTargetDesired(_msg);
      });

  topic = "position_target";
  // position_target_赋值
  position_target_sub_ = create_subscription<geometry_msgs::msg::PointStamped>(
      topic, 10,
      [this](const geometry_msgs::msg::PointStamped::SharedPtr _msg) {
        OnPositionTarget(_msg);
      });

  topic = "velocity_target";
  // velocity_target_赋值
  velocity_target_sub_ =
      create_subscription<geometry_msgs::msg::Vector3Stamped>(
          topic, 10,
          [this](const geometry_msgs::msg::Vector3Stamped::SharedPtr _msg) {
            OnVelocityTarget(_msg);
          });

  topic = "odometry";
  odometry_sub_ = create_subscription<nav_msgs::msg::Odometry>(
      topic, 10, [this](const nav_msgs::msg::Odometry::SharedPtr _msg) {
        OnOdometry(_msg);
      });
}

// 新增：目标姿态回调函数
void TrackingControllerBlueROVNode::OnAttitudeTargetDesired(
    const geometry_msgs::msg::QuaternionStamped::SharedPtr _msg) {
  hippo_common::convert::RosToEigen(_msg->quaternion, attitude_desired_);
  received_attitude_target_ = true;  // 添加标志位
}

void TrackingControllerBlueROVNode::UpdateControllerParams() {
  if (params_.updated) {
    params_.updated = false;
    controller_.SetPositionGain(params_.position_gain);
    controller_.SetVelocityGain(params_.velocity_gain);
  }
}

void TrackingControllerBlueROVNode::OnOdometry(
    const nav_msgs::msg::Odometry::SharedPtr _msg) {
  RCLCPP_INFO(get_logger(), "################  received_attitude_target_: %s", 
           received_attitude_target_ ? "true" : "false");
  // 添加状态检查
  if (!received_attitude_target_) {
    RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 2000, 
                         "Waiting for position and attitude targets");
    return;  // 如果没有收到必要的数据，直接返回
  }

  hippo_common::convert::RosToEigen(_msg->pose.pose.position, position_);
  hippo_common::convert::RosToEigen(_msg->twist.twist.linear, velocity_);
  hippo_common::convert::RosToEigen(_msg->pose.pose.orientation, attitude_);
  
  // 修改：设置期望状态，包括位置、速度和姿态
  controller_.SetDesiredState(position_target_, velocity_target_, attitude_desired_);

  UpdateControllerParams();
  
  // 修改：计算推力向量（在世界坐标系下）
  Eigen::Vector3d thrust_world = controller_.Update(position_, velocity_);
  // 将世界坐标系的推力转换到机体坐标系
  Eigen::Vector3d thrust_body = attitude_.inverse() * thrust_world;


  rclcpp::Time t_now = now();

  // 修改：发布目标姿态和推力向量
  PublishAttitudeTarget(attitude_desired_, thrust_body, t_now);
}


// 修改：更新发布函数，支持三维推力向量
void TrackingControllerBlueROVNode::PublishAttitudeTarget(
    const Eigen::Quaterniond &_attitude, const Eigen::Vector3d &thrust_body,
    const rclcpp::Time &_now) {
  if (attitude_target_pub_ == nullptr) {
    RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000,
                         "Publisher for attitude_target not available");
    return;
  }
  // 修改：使用新的消息类型
  auto msg = std::make_unique<hippo_control_msgs::msg::ThrustAttitudeTarget>();

  msg->header.stamp = _now;
  msg->header.frame_id = hippo_common::tf2_utils::frame_id::InertialFrame();
  // 把期望姿态_attitude直接赋值给msg
  hippo_common::convert::EigenToRos(_attitude, msg->attitude);


  msg->thrust.x = thrust_body.x();
  msg->thrust.y = thrust_body.y();
  msg->thrust.z = thrust_body.z();

  msg->mask = 0;
  
  // 设置角速度为零
  msg->body_rate.x = 0.0;
  msg->body_rate.y = 0.0;
  msg->body_rate.z = 0.0;

  // 发布目标四元数姿态 和 推力，所以可以转接姿态姿态控制器和推力控制器
  attitude_target_pub_->publish(std::move(msg));


}

}  // namespace trajectory_tracking
}  // namespace hippo_control

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(
    hippo_control::trajectory_tracking_bluerov::TrackingControllerBlueROVNode)
